/*
 *  Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
 *  Copyright (C) 2009, 2010, 2012 Austin Robot Technology, Jack O'Quin
 *
 *  License: Modified BSD Software License Agreement
 *
 *  $Id$
 */

#include <fstream>
#include <math.h>

#include <angles/angles.h>

#include "rawdata.h"


#define HLIPW(h) ( (((uint32_t)h << 24)&0xFF000000) | (((uint32_t)h << 8)&0x00FF0000) | (((uint32_t)h >> 8)&0x0000FF00) | (((uint32_t)h >> 24)&0x000000FF) )
namespace lidar209_rawdata
{
////////////////////////////////////////////////////////////////////////
//
// RawData base class implementation
//
////////////////////////////////////////////////////////////////////////

RawData::RawData(rclcpp::Node * private_nh)
  :private_nh_(private_nh)
{

  sn=0;
  tmp_detect=0;
  tmp_fpga=0;
  v_high=0;
  rpm=0;
  ver=0;
  return_num=0;
  th1=0;
  th2=0;
  th3=0;
  frq=0;
  mode1=0;
  mode2=0;
  saturation=0;//切换饱和与非饱和
  v_high_detect=0;//探测器高压截距
  tmp_v_high_detect=0;//探测器高压温度系数
  correction=0;//校正与否
  tmp_laser=0;//激光器温度
  UDP_port=0;//UDP端口号

}

/** Update parameters: conversions and update */
void RawData::setParameters(double min_range,
                            double max_range,
                            double view_direction,
                            double view_width)
{
  config_.min_range = min_range;
  config_.max_range = max_range;

  // converting angle parameters into the velodyne reference (rad)
  double tmp_min_angle = view_direction + view_width / 2;
  double tmp_max_angle = view_direction - view_width / 2;

  // computing positive modulo to keep theses angles into [0;2*M_PI]
  tmp_min_angle = ::fmod(::fmod(tmp_min_angle, 2 * M_PI) + 2 * M_PI, 2 * M_PI);
  tmp_max_angle = ::fmod(::fmod(tmp_max_angle, 2 * M_PI) + 2 * M_PI, 2 * M_PI);

  // converting into the hardware velodyne ref (negative yaml and degrees)
  // adding 0.5 performs a centered double to int conversion
  config_.min_angle = std::lround(100.0 * (2.0 * M_PI - tmp_min_angle) * 180.0 / M_PI);
  config_.max_angle = std::lround(100.0 * (2.0 * M_PI - tmp_max_angle) * 180.0 / M_PI);

  if (config_.min_angle == config_.max_angle) {
    // avoid returning empty cloud if min_angle = max_angle
    config_.min_angle = 0;
    config_.max_angle = 36000;
  }
}

/** Set up for on-line operation. */
int RawData::setup()
{

//  RCLCPP_INFO(private_nh_->get_logger(), "correction calibrationFile: %s", config_.calibrationFile.c_str());

  calibration_.read(config_.calibrationFile);
  if (!calibration_.initialized)
  {
    //  RCLCPP_INFO(this->get_logger(), "Unable to open calibration file:  %s", config_.calibrationFile.c_str());
    return -1;
  }

  config_.min_angle = 0;
  config_.max_angle = 36000;

  // config_.Rotation_Angle = rho_ang;
  // Set up cached values for sin and cos of all the possible headings
  for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index)
  {
    float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);
    cos_rot_table_[rot_index] = cosf(rotation);
    sin_rot_table_[rot_index] = sinf(rotation);
  }

  if (config_.model == "L209-2")
  {
    //InitTables_chaowu_64G();
    //两个激光点之间的水平角度偏差
    alpha_ontime = m_d_rpm * 0.00096;
  }
  else if (config_.model == "L209-72")
  {
    //InitTables_chaowu_CH72();
    config_.show_id = Num_Of_Detector_CH72;
    //    ROS_INFO_STREAM("show_id : " << Num_Of_Detector_CH72 );
    //发射两次激光的时间内，水平角度偏差。
    alpha_ontime = m_d_rpm/4 * 0.00015;
  }
  else if (config_.model == "L209-QS88")
  {
    InitTables_chaowu_CH88();
    config_.show_id = Num_Of_Detector_CH88;
    //    ROS_INFO_STREAM("show_id : " << Num_Of_Detector_CH88 );
    //发射两次激光的时间内，水平角度偏差。
    alpha_ontime = 360.0 * m_d_rpm /60.0 * 0.000014;
  }
  else if (config_.model == "L209-QS176")
  {

    alpha_ontime = 360.0 * m_d_rpm /60.0 * 0.000028;
    if (m_b_laser_hz){
      alpha_ontime = 360.0 * m_d_rpm /60.0 * 0.000014;
    }
    InitTables_chaowu_CH176();
  }
  else if (config_.model == "L209-ZW-QS176")
  {
    alpha_ontime = 360.0 * m_d_rpm /60.0 * 0.000028;
    if (m_b_laser_hz){
      alpha_ontime = 360.0 * m_d_rpm /60.0 * 0.000014;
    }
    InitTables_zw_CH176();

  }
  //  ROS_INFO_STREAM("alpha_ontime: " << alpha_ontime << ".");
  dis_tmp = 0.0;
  return 0;
}

int RawData::scansPerPacket() const
{
  return BLOCKS_PER_PACKET_CH88 * SCANS_PER_BLOCK_CH88;
}

int RawData::numLasers() const
{
  return calibration_.num_lasers;
}

void RawData::unpack_ch88(const lidar209_msgs::msg::Lidar209PacketCH88 &pkt,
                          DataContainerBase &pc)
{
  float t_cos_thita = 0;
  float t_sin_thita = 0;
  float t_t_thita = 0;
  float wendu = 0.0;
  int laser_number;
  int ROT_t=0;
  //ROS_INFO_STREAM("Received packet, time  88: " << pkt.stamp);

  const raw_packet_t_CH88 *raw = (const raw_packet_t_CH88 *)&pkt.data[0];

  recieved_time = HLIPW(raw->revolution);


  union two_bytes tmp1,tmp2;

  for (int i = 0; i < BLOCKS_PER_PACKET_CH88; i++)
  {
    tmp1.bytes[0] = raw->blocks[i].rotation[0];
    tmp1.bytes[1] = raw->blocks[i].rotation[1];
    int ROT = tmp1.uint;

    for (int j = 0, k = 0; j < SCANS_PER_BLOCK_CH88; j++, k += RAW_SCAN_SIZE)
    {
      float intensity;
      int laser_number = j ;
      int ring=(ROT/9000)*SCANS_PER_BLOCK_CH88+laser_number;
      intensity = raw->blocks[i].data[k + 2];

      if (
          (ROT >= config_.min_angle && ROT <= config_.max_angle && config_.min_angle < config_.max_angle) || (config_.min_angle > config_.max_angle && (ROT <= config_.max_angle || ROT >= config_.min_angle)))
      {

        float distance, dis0;

        distance =(raw->blocks[i].data[k]+raw->blocks[i].data[k+1]*255)* calibration_.distance_resolution_m;
        if(distance>200)
          continue;
        if(distance<10 && intensity<15 )
          continue;
        dis0 = distance;
        // distance=distance+dis_error_CH88[laser_number];

        distance=distance+calibration_.laser_corrections[laser_number].fix_offset;

        //------------------------------------------------------------
        if (pointInRange(distance))
        {
          // ROS_INFO_STREAM("distance, time  88: " << distance);
          VPoint point;
          //int alpha = ROT +alpha_ontime * fire_Serial_Time_CH88[laser_number] + config_.Rotation_Angle;
          int alpha = ROT +alpha_ontime * fire_Serial_Time_CH88[laser_number] + calibration_.Alpha_corr0;
          alpha=calib_angle(alpha);
          double deg_x,deg_y;
          CalculateXYZ_CH88(distance, alpha, laser_number,point,deg_x,deg_y);
          point.intensity =intensity;
          //                   if (point.y < 0.1 && point.y > -0.1 && point.x > 0 && point.x < 80.0 && point.z > -0.90 && point.z < 1.1) //dianxing -1.7525
          //         {
          //        point.intensity = 255;
          //    }

          point.range = distance;
          point.v_angle = deg_y;
          point.h_angle = deg_x;
          //正反放置切换
          if(m_b_updown)
          {
            point.y=-point.y;
            point.z=-point.z;
          }

          point.wendu = tmp_detect;
          point.point_time = recieved_time;
          int id_tmp=(alpha/9000)*SCANS_PER_BLOCK_CH88+laser_number;
          double dis_tmp=distance-dis_pre_CH88[id_tmp];
          double thrr_tmp=distance*0.005;
          if ((dis_tmp<-thrr_tmp ||dis_tmp>thrr_tmp) )
          {
            dis_pre_CH88[id_tmp]=distance;
            continue;
          }

          dis_pre_CH88[id_tmp]=distance;
          // pc.points.push_back(point);
          //  ++pc.width;
          pc.addPoint(
                point.x, point.y, point.z, point.laserid,
                point.range, point.intensity);
        }
      }
    }
    pc.newLine();
  }
}

void RawData::unpack_ch176(const lidar209_msgs::msg::Lidar209PacketCH88 &pkt,
                           DataContainerBase &pc)
{
  float t_cos_thita = 0;
  float t_sin_thita = 0;
  float t_t_thita = 0;
  float wendu = 0.0;
  int laser_number;
  int ROT_t=0;

  const raw_packet_t_CH88 *raw = (const raw_packet_t_CH88 *)&pkt.data[0];

  recieved_time = HLIPW(raw->revolution);
  int index=0;
  unsigned char  tt[4];
  unsigned char  tt1[6];//
  unsigned char  tt2[2];//
  unsigned char  tt3;//



  union two_bytes tmp1,tmp2;

  for (int i = 0; i < BLOCKS_PER_PACKET_CH88; i++)
  {
    unsigned int m_zy_flag=((raw->blocks[i].header&0x80)==0x80)?1:0;

    tmp1.bytes[0] = raw->blocks[i].rotation[0];
    tmp1.bytes[1] = raw->blocks[i].rotation[1];
    int ROT = tmp1.uint;

    for (int j = 0, k = 0; j < SCANS_PER_BLOCK_CH88; j++, k += RAW_SCAN_SIZE)
    {
      float intensity;
      int laser_number=j;
      int ring;
      int laser_group=laser_number;
      if(m_zy_flag)
      {
        ring=Num_Of_Detector_CH88+(ROT/9000)*SCANS_PER_BLOCK_CH88+laser_number;
        laser_group = 43-laser_number;
      }
      else
      {
        ring=(ROT/9000)*SCANS_PER_BLOCK_CH88+laser_number;
      }

      tmp2.bytes[0] = raw->blocks[i].data[k+1];
      tmp2.bytes[1] = raw->blocks[i].data[k];

      intensity = raw->blocks[i].data[k + 2];

      if (
          (ROT >= config_.min_angle && ROT <= config_.max_angle && config_.min_angle < config_.max_angle) || (config_.min_angle > config_.max_angle && (ROT <= config_.max_angle || ROT >= config_.min_angle)))
      {

        float distance, dis0;

        distance =(raw->blocks[i].data[k]+raw->blocks[i].data[k+1]*255)* calibration_.distance_resolution_m;
        if(distance>200)
          continue;
        if(distance<5 && intensity<15 )
          continue;
          if(distance>1.0 && distance<2.5 && intensity<60 )
          continue;
        dis0 = distance;
        distance=distance+calibration_.laser_corrections[laser_group].fix_offset;

        //------------------------------------------------------------
        if (pointInRange(distance))
        {
          // ROS_INFO_STREAM("distance, time  88: " << distance);
          VPoint point;
          // int alpha = ROT +alpha_ontime * fire_Serial_Time_CH176[laser_group] + config_.Rotation_Angle;
          int alpha = ROT +alpha_ontime * fire_Serial_Time_CH88[laser_number] + calibration_.Rotation_corr0;
          alpha=calib_angle(alpha);

          if(!interval_shielding(alpha,6300))
            continue;
          double deg_x,deg_y;
          CalculateXYZ_CH176(distance, alpha, laser_group,point,deg_x,deg_y,1-m_zy_flag);

          point.intensity =intensity;
          //                   if (point.y < 0.1 && point.y > -0.1 && point.x > 0 && point.x < 80.0 && point.z > -0.90 && point.z < 1.1) //dianxing -1.7525
          //         {
          //        point.intensity = 255;
          //    }

          point.range = distance;
          point.v_angle = deg_y;
          point.h_angle = deg_x;
          //正反放置切换
          if(m_b_updown)
          {
            point.y=-point.y;
            point.z=-point.z;
          }
          point.laserid=ring;
          point.wendu = ROT;
          point.point_time = recieved_time*0.5+fire_Serial_Time_CH88[laser_number]*2.315+13.889*i;

          double dis_tmp=distance-dis_pre_CH176[ring];
          double thrr_tmp=distance*0.007;
          if ((dis_tmp<-thrr_tmp ||dis_tmp>thrr_tmp) )
          {
            dis_pre_CH176[ring]=distance;
            continue;
          }
          dis_pre_CH176[ring]=distance;


          pc.addPoint(
                point.x, point.y, point.z, point.laserid,
                point.range, point.intensity);

        }
      }
    }
    pc.newLine();
  }
}

void RawData::CalculateXYZ_CH88(double range, int angle, int laser_ID, VPoint &pp, double &DegX, double &DegY)
{

  int alpha = 0;
  int beta = laser_angle_CH88[laser_ID];
  //ROS_INFO_STREAM("beta: " << beta);
  int Prism_ID=angle/9000;
  alpha= mirror_alpha[Prism_ID];//镜子仰角
  switch(Prism_ID)
  {
  case 0:
    angle = angle % 9000+ config_.ARR_Angle1;
    break;
  case 1:
    angle = angle % 9000+ config_.ARR_Angle2;
    break;
  case 2:
    angle = angle % 9000+ config_.ARR_Angle3;
    break;
  case 3:
    angle = angle % 9000+ config_.ARR_Angle4;
    break;
  }
  double D=0.02;
  double h=0.0215;
  double source_offset_x=0.01427;
  double source_offset_y=-0.048;


  double cos_theta=cal_cos(angle);
  double sin_theta=cal_sin(angle);
  double cos_alpha=cal_cos(alpha);
  double sin_alpha=cal_sin(alpha);
  double cos_beta=cal_cos(beta);
  double sin_beta=cal_sin(beta);


  double nx=cos_alpha*cos_theta;
  double ny=-cos_alpha*sin_theta;
  double nz=sin_alpha;
  double d=-cos_alpha*D-h*abs(sin_alpha)-0.0022; // 额外修正了镜子加工误差

  double Tx=source_offset_x;
  double Ty=range*cos_beta+source_offset_y;
  double Tz=range*sin_beta;

  double Tx_=(1-2*nx*nx)*Tx-2*ny*nx*Ty-2*nz*nx*Tz-2*d*nx;
  double  Ty_=-2*nx*ny*Tx+(1-2*ny*ny)*Ty-2*nz*ny*Tz-2*d*ny;
  double  Tz_=-2*nx*nz*Tx-2*ny*nz*Ty+(1-2*nz*nz)*Tz-2*d*nz;


  pp.x = Tx_;
  pp.y = Ty_;
  pp.z = Tz_;
  pp.laserid = laser_ID;// Prism_ID*22+laser_ID;
  return ;
}


void RawData::CalculateXYZ_CH176(double range, int angle, int laser_ID, VPoint &pp, double &DegX, double &DegY,int mode)
{
  // 假如laser_ID按0-43排
  // 计算模块
  int alpha = 0;
  int beta = laser_angle_CH88[laser_ID];
  int Prism_ID = angle/9000;
  if(mode)
  {// 模块二
    //Prism_ID = (Prism_ID-1) < 0 ? 3 : (Prism_ID-1);
    Prism_ID = (Prism_ID+1) > 3 ? 0 : (Prism_ID+1);
    switch(Prism_ID)
    {
    case 0:
      angle = angle % 9000+ calibration_.Alpha_corr11;
      break;
    case 1:
      angle = angle % 9000+ calibration_.Alpha_corr12;
      break;
    case 2:
      angle = angle % 9000+ calibration_.Alpha_corr13;
      break;
    case 3:
      angle = angle % 9000+ calibration_.Alpha_corr14;
      break;
    }
    beta=beta+calibration_.Beta_corr0;
    angle = angle - 9000; // 负角度方便统一公式 sin(-theta)=-sin(theta)
  }
  else
  {// 模块一
    angle=angle+calibration_.Alpha_corr0;
    switch(Prism_ID)
    {
    case 0:
      angle = angle % 9000+ calibration_.Alpha_corr21;
      break;
    case 1:
      angle = angle % 9000+ calibration_.Alpha_corr22;
      break;
    case 2:
      angle = angle % 9000+ calibration_.Alpha_corr23;
      break;
    case 3:
      angle = angle % 9000+ calibration_.Alpha_corr24;
      break;
    }
  }
  alpha= mirror_alpha[Prism_ID];//镜子仰角
  double D=0.02;
  double h=0.0215;
  double source_offset_x=0.01427;
  double source_offset_y=-0.048;


  double cos_theta=cal_cos(angle);
  double sin_theta=cal_sin(angle);
  double cos_alpha=cal_cos(alpha);
  double sin_alpha=cal_sin(alpha);
  double cos_beta=cal_cos(beta);
  double sin_beta=cal_sin(beta);


  double nx=cos_alpha*cos_theta;
  double ny=-cos_alpha*sin_theta;
  double nz=sin_alpha;
  double d=-cos_alpha*D-h*abs(sin_alpha)-0.0022; // 额外修正了镜子加工误差

  double Tx=source_offset_x;
  double Ty;
  if(mode)
  {//模块二
    Ty=-range*cos_beta+source_offset_y;
  }
  else
  {//模块一
    Ty=range*cos_beta-source_offset_y;
  }
  double Tz=range*sin_beta;

  double Tx_=(1-2*nx*nx)*Tx-2*ny*nx*Ty-2*nz*nx*Tz-2*d*nx;
  double Ty_=-2*nx*ny*Tx+(1-2*ny*ny)*Ty-2*nz*ny*Tz-2*d*ny;
  double Tz_=-2*nx*nz*Tx-2*ny*nz*Ty+(1-2*nz*nz)*Tz-2*d*nz;

  pp.x = Tx_;
  pp.y = Ty_;
  pp.z = Tz_;
  DegX=Prism_ID;
  DegY=beta;
  pp.laserid = laser_ID;// Prism_ID*22+laser_ID;

  return ;
}

int RawData::calib_angle(int ang)
{
  while (ang < 0)
  {
    ang = ang + 36000;
  }

  if (ang > 35999)
  {
    ang = ang % 36000;
  }
  return ang;
}
double RawData::cal_cos(int ang)
{
  return cos_rot_table_[calib_angle(ang)];
}
double RawData::cal_sin(int ang)
{

  return sin_rot_table_[calib_angle(ang)];
}
//

bool RawData::interval_shielding(int current_angle, unsigned int angle_view)
{
  //1:1350 ---->7650
  //2:10350 --->16650
  //3:19350 --->25650
  //4:28350 --->34650
  if (((unsigned)(current_angle - 1350) <= angle_view)  ||
      ((unsigned)(current_angle - 10350) <= angle_view)  ||
      ((unsigned)(current_angle - 19350) <= angle_view)  ||
      ((unsigned)(current_angle - 28350) <= angle_view) )
    return true;
  else
    return false;
}
int RawData::bytes2ToInt(unsigned char  *b)
{
  int addr = b[1];
  addr |= ((b[0] << 8) & 0xFF00);
  return addr;
}

int RawData::bytesToInt(unsigned char  b)
{
  int addr = b & 0xFF;
  return addr;
}


void RawData::InitTables_chaowu_CH88()
{

  //double t[SCANS_PER_BLOCK_CH88] = { -12.512,-11.04,-9.568,-8.096,-6.624,-5.152,-3.68,-2.208,-0.736,0.736,2.208,3.68,5.152,6.624,8.096,9.568,11.04,12.512,12.512,12.512,12.512,12.512};
  for (int i=0;i<SCANS_PER_BLOCK_CH88;i++)//起始角度-14.397，间隔1.379
  {
    laser_angle_CH88[i]=-1457+i*138;
  }
  mirror_alpha[0]=-12;
  mirror_alpha[1]=-37;
  mirror_alpha[2]=12;
  mirror_alpha[3]=37;

  mirror_alpha_error[0]=0;
  mirror_alpha_error[1]=0;
  mirror_alpha_error[2]=0;
  mirror_alpha_error[3]=0;
  memset(dis_error_CH88, 0, sizeof(double) * SCANS_PER_BLOCK_CH88);

  //    double d[18] = { -4.4385,-4.3560,-4.5998,-4.4010,-4.4873,-4.6335,
  //-4.4935,-4.4235,-4.4423,-4.4966,-4.5985,-4.6148,
  //-4.6260,-4.6485,-4.6610,-4.6435,-4.6885,-4.7060};//1#

  

  // memcpy(dis_error,d,sizeof(double)*18);

  /////  l209

  memset(alpha_CH88, 0, sizeof(int) * Num_Of_Detector_CH88);
  memset(thita_CH88, 0, sizeof(int) * Num_Of_Detector_CH88);
  memset(cos_thita_value_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(sin_thita_value_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(R_corr_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(Z_corr_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(fire_Serial_Time_CH88, 0, sizeof(unsigned int) * SCANS_PER_BLOCK_CH88);
  memset(dis_pre_CH88, 0, sizeof(double) * Num_Of_Detector_CH88);

  fire_Serial_Time_CH88[0] = 0;
  fire_Serial_Time_CH88[6] = 0;
  fire_Serial_Time_CH88[11] = 0;
  fire_Serial_Time_CH88[17] = 0;

  float index = 0;
  for (int i = 0; i < 6; i++)//将激光器分为4组，每组6个
  {
    fire_Serial_Time_CH88[i] = index;
    fire_Serial_Time_CH88[i+11] = index;
    index+=1.0;
  }

  index = 0;
  for (int i = 0; i < 5; i++)//将激光器分为4组，每组6个
  {
    fire_Serial_Time_CH88[i+6] = index;
    fire_Serial_Time_CH88[i+17] = index;
    index+=1.0;
  }

}

void RawData::InitTables_chaowu_CH176()
{

  memset(alpha_CH88, 0, sizeof(int) * Num_Of_Detector_CH88);
  memset(thita_CH88, 0, sizeof(int) * Num_Of_Detector_CH88);
  memset(cos_thita_value_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(sin_thita_value_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(R_corr_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(Z_corr_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(fire_Serial_Time_CH88, 0, sizeof(unsigned int) * SCANS_PER_BLOCK_CH88);
  memset(fire_Serial_Time_CH176, 0, sizeof(unsigned int) * SCANS_PER_BLOCK_CH176);

  memset(dis_pre_CH176, 0, sizeof(double) * Num_Of_Detector_CH176);

  memset(laser_angle_CH88, 0, sizeof(double) * SCANS_PER_BLOCK_CH176);

  for (int i=0;i<SCANS_PER_BLOCK_CH88;i++)//模块一(0-21)起始角度-14.389，间隔1.379
  {
    laser_angle_CH88[i]=(-14389+i*1379)/10;
    //  ROS_INFO_STREAM("laser_angle_CH88 beta: " << laser_angle_CH88[i]);
  }
  for (int i=0;i<SCANS_PER_BLOCK_CH88;i++)//模块二(22-43)起始角度-14.569，间隔1.379
  {
    laser_angle_CH88[i+22]=(-14569+i*1379)/10;
    // ROS_INFO_STREAM("laser_angle_CH88 beta: " << laser_angle_CH88[i]);
  }
  mirror_alpha[0]=-12;
  mirror_alpha[1]=-37;
  mirror_alpha[2]=12;
  mirror_alpha[3]=37;

  mirror_alpha_error[0]=0;
  mirror_alpha_error[1]=0;
  mirror_alpha_error[2]=0;
  mirror_alpha_error[3]=0;
  memset(dis_error_CH88, 0, sizeof(double) * SCANS_PER_BLOCK_CH88);

  fire_Serial_Time_CH88[0] = 0;
  fire_Serial_Time_CH88[6] = 0;
  fire_Serial_Time_CH88[11] = 0;
  fire_Serial_Time_CH88[17] = 0;

  float index = 0;
  for (int i = 0; i < 6; i++)//将激光器分为4组，每组6个
  {
    fire_Serial_Time_CH176[i] = index;
    fire_Serial_Time_CH176[i+11] = index;
    index+=1.0;
  }

  index = 0;
  for (int i = 0; i < 5; i++)//将激光器分为4组，每组6个
  {
    fire_Serial_Time_CH176[i+6] = index;
    fire_Serial_Time_CH176[i+17] = index;
    index+=1.0;
  }

  index = 6;
  for (int i = 0; i < 6; i++)//将激光器分为4组，每组6个
  {
    fire_Serial_Time_CH176[43-i] = index;
    fire_Serial_Time_CH176[43-11-i] = index;
    index+=1.0;
  }

  index = 6;
  for (int i = 0; i < 5; i++)//将激光器分为4组，每组6个
  {
    fire_Serial_Time_CH176[37-i] = index;
    fire_Serial_Time_CH176[37-11-i] = index;
    index+=1.0;
  }

}
void RawData::InitTables_zw_CH176()
{

  memset(alpha_CH88, 0, sizeof(int) * Num_Of_Detector_CH88);
  memset(thita_CH88, 0, sizeof(int) * Num_Of_Detector_CH88);
  memset(cos_thita_value_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(sin_thita_value_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(R_corr_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(Z_corr_CH88, 0, sizeof(float) * Num_Of_Detector_CH88);
  memset(fire_Serial_Time_CH88, 0, sizeof(unsigned int) * SCANS_PER_BLOCK_CH88);
  memset(fire_Serial_Time_CH176, 0, sizeof(unsigned int) * SCANS_PER_BLOCK_CH176);
  memset(dis_pre_CH176, 0, sizeof(double) * Num_Of_Detector_CH176);

  memset(laser_angle_CH88, 0, sizeof(double) * SCANS_PER_BLOCK_CH176);

  for (int i=0;i<SCANS_PER_BLOCK_CH88;i++)//模块一(0-21)起始角度-9.389，间隔1.379
  {
    laser_angle_CH88[i]=(-939+i*138);
   // ROS_INFO_STREAM("laser_angle_CH88 beta: " << laser_angle_CH88[i]);
  }
  for (int i=0;i<SCANS_PER_BLOCK_CH88;i++)//模块二(22-43)起始角度-19.569，间隔1.379
  {
    laser_angle_CH88[i+22]=(-1957+i*138);
  //  ROS_INFO_STREAM("laser_angle_CH88 beta: " << laser_angle_CH88[i+22]);
  }
  mirror_alpha[0]=-12;
  mirror_alpha[1]=-37;
  mirror_alpha[2]=12;
  mirror_alpha[3]=37;


  mirror_alpha_error[0]=0;
  mirror_alpha_error[1]=0;
  mirror_alpha_error[2]=0;
  mirror_alpha_error[3]=0;
  memset(dis_error_CH88, 0, sizeof(double) * SCANS_PER_BLOCK_CH88);

  fire_Serial_Time_CH88[0] = 0;
  fire_Serial_Time_CH88[6] = 0;
  fire_Serial_Time_CH88[11] = 0;
  fire_Serial_Time_CH88[17] = 0;

  float index = 0;
  for (int i = 0; i < 6; i++)//将激光器分为4组，每组6个
  {
    fire_Serial_Time_CH176[i] = index;
    fire_Serial_Time_CH176[i+11] = index;
    index+=1.0;
  }

  index = 0;
  for (int i = 0; i < 5; i++)//将激光器分为4组，每组6个
  {
    fire_Serial_Time_CH176[i+6] = index;
    fire_Serial_Time_CH176[i+17] = index;
    index+=1.0;
  }

  index = 6;
  for (int i = 0; i < 6; i++)//将激光器分为4组，每组6个
  {
    fire_Serial_Time_CH176[43-i] = index;
    fire_Serial_Time_CH176[43-11-i] = index;
    index+=1.0;
  }

  index = 6;
  for (int i = 0; i < 5; i++)//将激光器分为4组，每组6个
  {
    fire_Serial_Time_CH176[37-i] = index;
    fire_Serial_Time_CH176[37-11-i] = index;

    index+=1.0;
  }
}
} // namespace lidar209_rawdata
